#include <tT.h>
#include <env.h>

#include "dtc.h"
#include "mcpwm.h"
#include "hsadc.h"


env_result_t dtc_init(dtc_t *self, void *none)
{
	int32_t i;
	int32_t data[32];
  ENV_DEBUG("enter dtc_init()\r\n");
	
	CUR_RESET();	// Reset the Disable latch on the ADCs so we can drive the mosfets.

	// Pull the V-leg low and the W-leg high
	GPIO1->FIOCLR =	(1<<19) | 
				(1<<22) | 
				(1<<26) | 
				(1<<28) | 
				(1<<29);

	d_delay(2000);	// Wait 2000 clock cykles
	
	GPIO1->FIOSET = (1<<25) | (1<<29);	//MCOB2;
	

	/* Set ports to output when in general mode p.123 */
	GPIO1->FIODIR |= 	(1<<19) |  	//MCOA0
				(1<<22) | 	//MCOB0
				(1<<25) | 	//MCOA1
				(1<<26) | 	//MCOB1
				(1<<28) ; 	//MCOA2
				
	d_delay(20000);	// Wait 200 clock cykles			
	GPIO1->FIODIR |= 	(1<<19) |  	//MCOA0
				(1<<22) | 	//MCOB0
				(1<<25) | 	//MCOA1
				(1<<26) | 	//MCOB1
				(1<<28) | 	//MCOA2
				(1<<29) ;		//MC0B2
				
	d_delay(2000);	// Wait 2000 clock cykles			
				
	/* Start calibrating, reset the counters and start sampling. */
	PWM1->TCR = 0;			// Stop counters
	TIM0->TCR = 0;
	PWM1->TC = 0;				// Reset counters
	TIM0->TC = 0;
	TIM0->IR = (1<<0);	// Clear Match Flag
	PWM1->TCR = 1;			// Enable the counters
	TIM0->TCR = 1;	
	GPIO1->FIOSET = (1<<28);
	for ( i = 0 ; i < 8 ; i++ )
	{
		while (!(TIM0->IR & (1<<0))) {
			; // Wait until the conversion is done.
		}
		PWM1->TCR = 0;	
		data[i]= PWM1->TC;
  	PWM1->TC = 0;
		PWM1->TCR = 1;
		TIM0->IR |= (1<<0);		
	}	
	
	GPIO1->FIOCLR =	(1<<19) | 
				(1<<22) | 
				(1<<25) |
				(1<<26) | 
				(1<<28) | 
				(1<<29);
	
	for (i=0 ; i < 8; i++) {
		env_send(data[i]);
		ENV_DEBUG("\r\n");
	}
	
	return 0;
}

